package simulator.main;

import java.awt.Graphics;
import java.awt.event.KeyEvent;
import java.awt.event.KeyListener;
import java.awt.image.BufferedImage;
import java.io.FileInputStream;
import java.io.IOException;
import java.io.InputStream;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import java.util.Queue;
import java.util.Vector;

import javax.imageio.ImageIO;
import javax.swing.*;

import simulator.entities.BaseTrafficlight;
import simulator.entities.BaseVehicle;
import simulator.entities.BikeTrafficLight;
import simulator.entities.BusTrafficLight;
import simulator.entities.CarTrafficLight;
import simulator.entities.PedTrafficLight;
import simulator.entities.TrafficLane;
import simulator.enums.Direction;
import simulator.enums.LaneType;
import simulator.enums.LightState;
import simulator.enums.Loop;
import simulator.jsoncode.DecodeCallback;
import simulator.jsoncode.JSONDecoder;
import simulator.jsoncode.JSONEncoder;
import simulator.physengine.Physengine;
import simulator.drawengine.DrawEngine;
import simulator.types.Lane;
import simulator.types.Vector2f;
import simulator.types.VehicleInput;

import net.minidev.json.JSONArray;
import net.minidev.json.JSONObject;
import net.minidev.json.parser.JSONParser;
import net.minidev.json.parser.ParseException;

import JTCPconnection.IMessageReceiver;
import JTCPconnection.TCPconnection;
import JTCPconnection.TCPconnectionException;

/**
 * Dummy simulator - provides basic functionality in order to test controller
 * 
 * inputs JSON input file from "C:\input.json"
 * 
 * The simulator can only run for 292 real years, after that the timer will not function correctly.
 * 
 * 
 * Assuming (for now): - total simulation area = 2x2 km - intersection center = (0, 0) - intersection width, height: 27m - lane width = 3m
 * 
 * 
 * @author M-R-D
 * 
 */
public class Simulator extends JPanel implements IMessageReceiver, DecodeCallback, KeyListener
{
	//Assumptions
	private static final float			LANE_LENGTH				= 125;
	private static final float			LANE_WIDTH				= 4.5f;
	private static final float			LANE_COUNT				= 9;
	private static final float			FAR_LOOP_DISTANCE		= 60;
	private static final float			NEAR_LOOP_DISTANCE		= -4f;
	private static final float			STOP_DISTANCE			= -4f;
	private static final float			TRAFFICLIGHT_DISTANCE	= -5f;

	//inferred constants
	private static final float			INTERSECTION_SIZE		= LANE_COUNT * LANE_WIDTH;
	private static final float			FAR_LOOP_OFFSET			= (INTERSECTION_SIZE / 2) + FAR_LOOP_DISTANCE;
	private static final float			NEAR_LOOP_OFFSET		= (INTERSECTION_SIZE / 2) + NEAR_LOOP_DISTANCE;
	private static final float			STOP_OFFSET				= (INTERSECTION_SIZE / 2) + STOP_DISTANCE;
	private static final float			TRAFFICLIGHT_OFFSET		= (INTERSECTION_SIZE / 2) + TRAFFICLIGHT_DISTANCE;

	//config fields
	private static final String			IPADDRESS_KEY			= "ipaddress";
	private static final String			INPUTFILE_KEY			= "inputfile";
	private static final String			MULTIPLIER_KEY			= "multiplier";

	static String						CONTROLLER_ADDRESS		= "127.0.0.1";
	static final int					CONTROLLER_PORT			= 1337;

	static final String					CONFIG_LOCATION			= "config.json";
	static String						INPUT_FILE_LOCATION		= "E:\\input.json";

	static final String					start_msg				= "[\n" + "\t{\"starttime\":\"21:30\"},\n" + "\t{\"multiplier\":1}\n" + "]";

	TCPconnection						_tcpcon;
	JSONParser							_parser;

	Queue<VehicleInput>					_inputs;

	private long						_previousTime;
	private double						_elapsedTime;
	private int							_multiplier				= 1;
	private int							_multiplierPow			= 0;

	private boolean						_terminate				= false;

	private Physengine					_physics;
	private DrawEngine					_drawings;

	private Queue<String>				_tcpMessages;

	private Map<String, TrafficLane>	_lanes;

	private BufferedImage				_bgImage;

	public Simulator()
	{
		_parser = new JSONParser(JSONParser.DEFAULT_PERMISSIVE_MODE);
		loadConfig();

		loadSimulationFile();
		startConnection();
		_physics = new Physengine();
		_drawings = new DrawEngine();
		_tcpMessages = new LinkedList<>();

		_lanes = new HashMap<>();
		setupLanes();
	}

	private void loadConfig()
	{
		try (FileInputStream is = new FileInputStream(CONFIG_LOCATION))
		{
			Object result = _parser.parse(is);

			System.out.println(result.getClass() + " : " + result);

			if (result instanceof JSONObject)
			{
				JSONObject config = (JSONObject) result;

				CONTROLLER_ADDRESS = (String) config.get(IPADDRESS_KEY);
				INPUT_FILE_LOCATION = (String) config.get(INPUTFILE_KEY);
				_multiplier = (int) config.get(MULTIPLIER_KEY);
			}
			else
			{
				System.out.println("Expected JSON Object as config root element");
				//System.exit(0);
			}
		}
		catch (IOException e)
		{
			e.printStackTrace();
			//System.exit(0);
		}
		catch (ParseException e)
		{
			System.out.println("Not able to parse config json file");
			e.printStackTrace();
			//System.exit(0);
		}
	}

	private void setupLanes()
	{
		Lane l;
		BaseTrafficlight light;
		TrafficLane lane;

		/*
		 * north
		 */

		//Lane 0: first sidewalk
		l = new Lane(Direction.NORTH, LaneType.SIDEWALK_RIGHTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-3.1f * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(-3.1f * LANE_WIDTH, FAR_LOOP_OFFSET), nv(-3.1f * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(-3.1f * LANE_WIDTH, STOP_OFFSET), nv(-3.1f * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);

		_drawings.add(lane);

		//Lane 1: bike lane
		l = new Lane(Direction.NORTH, LaneType.BIKELANE);
		light = new BikeTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-2.7f * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(-2.7f * LANE_WIDTH, FAR_LOOP_OFFSET), nv(-2.7f * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(-2.7f * LANE_WIDTH, STOP_OFFSET), nv(-2.7f * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 2: bus lane
		l = new Lane(Direction.NORTH, LaneType.BUSLANE);
		light = new BusTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-2 * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(-2 * LANE_WIDTH, FAR_LOOP_OFFSET), nv(-2 * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(-2 * LANE_WIDTH, STOP_OFFSET), nv(-2 * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 3: car right lane
		l = new Lane(Direction.NORTH, LaneType.CAR_RIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-1 * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(-1 * LANE_WIDTH, FAR_LOOP_OFFSET), nv(-1 * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(-1 * LANE_WIDTH, STOP_OFFSET), nv(-1 * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 4: car straight lane
		l = new Lane(Direction.NORTH, LaneType.CAR_STRAIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(0 * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(0 * LANE_WIDTH, FAR_LOOP_OFFSET), nv(0 * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(0 * LANE_WIDTH, STOP_OFFSET), nv(0 * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 5: car straight lane
		l = new Lane(Direction.NORTH, LaneType.CAR_LEFT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(1 * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(1 * LANE_WIDTH, FAR_LOOP_OFFSET), nv(1 * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(1 * LANE_WIDTH, STOP_OFFSET), nv(1 * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 6: car exit (no spawn or trafficlight)
		l = new Lane(Direction.NORTH, LaneType.VEHICLE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(2 * LANE_WIDTH, LANE_LENGTH), mulpi(0.5f), null, null, nv(2 * LANE_WIDTH, STOP_OFFSET), null);
		_lanes.put(l.toString(), lane);

		//Lane 8: bike exit (no spawn or trafficlight)
		l = new Lane(Direction.NORTH, LaneType.BIKE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(2.7f * LANE_WIDTH, LANE_LENGTH), mulpi(0.5f), null, null, nv(2.7f * LANE_WIDTH, STOP_OFFSET), null);
		_lanes.put(l.toString(), lane);

		//Lane 7: second sidewalk
		l = new Lane(Direction.NORTH, LaneType.SIDEWALK_LEFTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(3.1f * LANE_WIDTH, LANE_LENGTH), mulpi(1.5f), nv(3.1f * LANE_WIDTH, FAR_LOOP_OFFSET), nv(3.1f * LANE_WIDTH, NEAR_LOOP_OFFSET), nv(3.1f * LANE_WIDTH, STOP_OFFSET), nv(3.1f * LANE_WIDTH, TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		/*
		 * west
		 */

		//Lane 0: first sidewalk
		l = new Lane(Direction.WEST, LaneType.SIDEWALK_RIGHTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, -3.1f * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, -3.1f * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, -3.1f * LANE_WIDTH), nv(-STOP_OFFSET, -3.1f * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, -3.1f * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 1: bike lane
		l = new Lane(Direction.WEST, LaneType.BIKELANE);
		light = new BikeTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, -2.7f * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, -2.7f * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, -2.7f * LANE_WIDTH), nv(-STOP_OFFSET, -2.7f * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, -2.7f * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 2: bus lane
		l = new Lane(Direction.WEST, LaneType.BUSLANE);
		light = new BusTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, -2 * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, -2 * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, -2 * LANE_WIDTH), nv(-STOP_OFFSET, -2 * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, -2 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 3: car right lane
		l = new Lane(Direction.WEST, LaneType.CAR_RIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, -1 * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, -1 * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, -1 * LANE_WIDTH), nv(-STOP_OFFSET, -1 * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, -1 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 4: car straight lane
		l = new Lane(Direction.WEST, LaneType.CAR_STRAIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, 0 * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, 0 * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, 0 * LANE_WIDTH), nv(-STOP_OFFSET, 0 * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, 0 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 5: car straight lane
		l = new Lane(Direction.WEST, LaneType.CAR_LEFT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, 1 * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, 1 * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, 1 * LANE_WIDTH), nv(-STOP_OFFSET, 1 * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, 1 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 6: car exit (no spawn or trafficlight)
		l = new Lane(Direction.WEST, LaneType.VEHICLE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, 2 * LANE_WIDTH), mulpi(0f), null, null, nv(-STOP_OFFSET, 2 * LANE_WIDTH), null);
		_lanes.put(l.toString(), lane);

		//Lane 8: bike exit (no spawn or trafficlight)
		l = new Lane(Direction.WEST, LaneType.BIKE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, 2.7f * LANE_WIDTH), mulpi(0f), null, null, nv(-STOP_OFFSET, 2.7f * LANE_WIDTH), null);
		_lanes.put(l.toString(), lane);

		//Lane 7: second sidewalk
		l = new Lane(Direction.WEST, LaneType.SIDEWALK_LEFTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-LANE_LENGTH, 3.1f * LANE_WIDTH), mulpi(1f), nv(-FAR_LOOP_OFFSET, 3.1f * LANE_WIDTH), nv(-NEAR_LOOP_OFFSET, 3.1f * LANE_WIDTH), nv(-STOP_OFFSET, 3.1f * LANE_WIDTH), nv(-TRAFFICLIGHT_OFFSET, 3.1f * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		/*
		 * south
		 */

		//Lane 0: first sidewalk
		l = new Lane(Direction.SOUTH, LaneType.SIDEWALK_RIGHTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(3.1f * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(3.1f * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(3.1f * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(3.1f * LANE_WIDTH, -STOP_OFFSET), nv(3.1f * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 1: bike lane
		l = new Lane(Direction.SOUTH, LaneType.BIKELANE);
		light = new BikeTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(2.7f * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(2.7f * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(2.7f * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(2.7f * LANE_WIDTH, -STOP_OFFSET), nv(2.7f * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 2: bus lane
		l = new Lane(Direction.SOUTH, LaneType.BUSLANE);
		light = new BusTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(2 * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(2 * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(2 * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(2 * LANE_WIDTH, -STOP_OFFSET), nv(2 * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 3: car right lane
		l = new Lane(Direction.SOUTH, LaneType.CAR_RIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(1 * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(1 * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(1 * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(1 * LANE_WIDTH, -STOP_OFFSET), nv(1 * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 4: car straight lane
		l = new Lane(Direction.SOUTH, LaneType.CAR_STRAIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(0 * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(0 * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(0 * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(0 * LANE_WIDTH, -STOP_OFFSET), nv(0 * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 5: car straight lane
		l = new Lane(Direction.SOUTH, LaneType.CAR_LEFT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-1 * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(-1 * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(-1 * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(-1 * LANE_WIDTH, -STOP_OFFSET), nv(-1 * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 6: car exit (no spawn or trafficlight)
		l = new Lane(Direction.SOUTH, LaneType.VEHICLE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(-2 * LANE_WIDTH, -LANE_LENGTH), mulpi(1.5f), null, null, nv(-2 * LANE_WIDTH, -STOP_OFFSET), null);
		_lanes.put(l.toString(), lane);

		//Lane 8: bike exit (no spawn or trafficlight)
		l = new Lane(Direction.SOUTH, LaneType.BIKE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(-2.7f * LANE_WIDTH, -LANE_LENGTH), mulpi(1.5f), null, null, nv(-2.7f * LANE_WIDTH, -STOP_OFFSET), null);
		_lanes.put(l.toString(), lane);

		//Lane 7: second sidewalk
		l = new Lane(Direction.SOUTH, LaneType.SIDEWALK_LEFTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(-3.1f * LANE_WIDTH, -LANE_LENGTH), mulpi(0.5f), nv(-3.1f * LANE_WIDTH, -FAR_LOOP_OFFSET), nv(-3.1f * LANE_WIDTH, -NEAR_LOOP_OFFSET), nv(-3.1f * LANE_WIDTH, -STOP_OFFSET), nv(-3.1f * LANE_WIDTH, -TRAFFICLIGHT_OFFSET));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		/*
		 * west
		 */

		//Lane 0: first sidewalk
		l = new Lane(Direction.EAST, LaneType.SIDEWALK_RIGHTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, 3.1f * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, 3.1f * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, 3.1f * LANE_WIDTH), nv(STOP_OFFSET, 3.1f * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, 3.1f * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 1: bike lane
		l = new Lane(Direction.EAST, LaneType.BIKELANE);
		light = new BikeTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, 2.7f * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, 2.7f * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, 2.7f * LANE_WIDTH), nv(STOP_OFFSET, 2.7f * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, 2.7f * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 2: bus lane
		l = new Lane(Direction.EAST, LaneType.BUSLANE);
		light = new BusTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, 2 * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, 2 * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, 2 * LANE_WIDTH), nv(STOP_OFFSET, 2 * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, 2 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 3: car right lane
		l = new Lane(Direction.EAST, LaneType.CAR_RIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, 1 * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, 1 * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, 1 * LANE_WIDTH), nv(STOP_OFFSET, 1 * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, 1 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 4: car straight lane
		l = new Lane(Direction.EAST, LaneType.CAR_STRAIGHT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, 0 * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, 0 * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, 0 * LANE_WIDTH), nv(STOP_OFFSET, 0 * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, 0 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 5: car straight lane
		l = new Lane(Direction.EAST, LaneType.CAR_LEFT);
		light = new CarTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, -1 * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, -1 * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, -1 * LANE_WIDTH), nv(STOP_OFFSET, -1 * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, -1 * LANE_WIDTH));
		_lanes.put(l.toString(), lane);
		_drawings.add(lane);

		//Lane 6: car exit (no spawn or trafficlight)
		l = new Lane(Direction.EAST, LaneType.VEHICLE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, -2 * LANE_WIDTH), mulpi(1f), null, null, nv(STOP_OFFSET, -2 * LANE_WIDTH), null);
		_lanes.put(l.toString(), lane);

		//Lane 8: bike exit (no spawn or trafficlight)
		l = new Lane(Direction.EAST, LaneType.BIKE_EXIT);
		light = null;
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, -2.7f * LANE_WIDTH), mulpi(1f), null, null, nv(STOP_OFFSET, -2.7f * LANE_WIDTH), null);
		_lanes.put(l.toString(), lane);

		//Lane 7: second sidewalk
		l = new Lane(Direction.EAST, LaneType.SIDEWALK_LEFTSIDE);
		light = new PedTrafficLight();
		_physics.add(light);
		//
		lane = new TrafficLane(this, l, light, nv(LANE_LENGTH, -3.1f * LANE_WIDTH), mulpi(0f), nv(FAR_LOOP_OFFSET, -3.1f * LANE_WIDTH), nv(NEAR_LOOP_OFFSET, -3.1f * LANE_WIDTH), nv(STOP_OFFSET, -3.1f * LANE_WIDTH), nv(TRAFFICLIGHT_OFFSET, -3.1f * LANE_WIDTH));
		_drawings.add(lane);
		_lanes.put(l.toString(), lane);

	}

	private static Vector2f nv(float x, float y)
	{
		return new Vector2f(x, y);
	}

	private static float mulpi(float mul)
	{
		return (float) (Math.PI * mul);
	}

	private void loadSimulationFile()
	{
		try (FileInputStream is = new FileInputStream(INPUT_FILE_LOCATION))
		{
			Object result = _parser.parse(is);

			if (result instanceof JSONArray)
			{
				processJSONInput((JSONArray) result);
			}
			else
			{
				System.out.println("Expected JSON Array as root element");
				System.exit(0);
			}
		}
		catch (IOException e)
		{
			e.printStackTrace();
			System.exit(0);
		}
		catch (ParseException e)
		{
			System.out.println("Not able to parse input json file");
			e.printStackTrace();
			System.exit(0);
		}
	}

	private void processJSONInput(JSONArray root)
	{
		List<VehicleInput> _usInputs = new ArrayList<VehicleInput>();

		for (int i = 0; i < root.size(); i++)
		{
			Object o = root.get(i);

			if (o instanceof JSONObject)
			{
				try
				{
					VehicleInput input = new VehicleInput((JSONObject) o, this);
					_usInputs.add(input);
				}
				catch (IllegalArgumentException e)
				{
					System.out.println("Expected only Vehicle Input Objects");
					System.exit(0);
				}
			}
			else
			{
				System.out.println("Expected only JSONObject in root JSON Array");
				System.exit(0);
			}
		}

		java.util.Collections.sort(_usInputs);

		_inputs = new LinkedList<>();

		for (int i = 0; i < _usInputs.size(); i++)
		{
			_inputs.add(_usInputs.get(i));
		}
	}

	private void startConnection()
	{
		_tcpcon = new TCPconnection(CONTROLLER_PORT, this);

		try
		{
			_tcpcon.connect(CONTROLLER_ADDRESS);
		}
		catch (TCPconnectionException e)
		{
			System.out.println("Connect: failed");
			e.printStackTrace();
			System.exit(0);
		}
		catch (Exception e)
		{
			System.out.println("Send: failed");
			e.printStackTrace();
			System.exit(0);
		}
	}

	@Override
	public void notifyConnected()
	{
		System.out.println("Was connected");
	}

	@Override
	public void receiveMessage(String arg0)
	{
		System.out.println("rec: " + arg0);

		synchronized (_tcpMessages)
		{
			_tcpMessages.add(arg0);
		}
	}

	@Override
	public void notifyDisconnected()
	{
		System.out.println("disconnected");
	}

	@Override
	public void notifyConnectionLost()
	{
		System.err.println("connection lost");
		System.exit(0);
	}

	public void paintComponent(Graphics g)
	{
		super.paintComponent(g);

		System.out.println("paint");
	}

	public void start()
	{
		try
		{
			JSONObject startTime = new JSONObject();
			startTime.put("starttime", "11:30");
			JSONObject multiplierObj = new JSONObject();
			multiplierObj.put("multiplier", _multiplier);

			_tcpcon.send("[" + startTime.toJSONString() + "]");
			_tcpcon.send("[" + multiplierObj.toJSONString() + "]");
		}
		catch (Exception e)
		{
			e.printStackTrace();
		}

		JFrame f = new JFrame("Simulatie");
		f.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		//f.setResizable(false);
		f.setSize(816, 838);
		f.add(_drawings);
		f.setVisible(true);

		f.addKeyListener(this);

		mainloop();
	}

	private void mainloop()
	{
		_elapsedTime = 0;
		_previousTime = System.nanoTime();
		while (true)
		{
			long currentTime = System.nanoTime();
			float dt = ((currentTime - _previousTime) / 1000000000f) * _multiplier; /* elapsed time in sec since last iteration */
			_previousTime = currentTime;
			_elapsedTime += dt;

			//System.out.println("Tick/ " + _elapsedTime + " / (+" + dt + ")");

			checkMessages();
			checkSpawn(_elapsedTime);
			_physics.performIteration(dt);

			_drawings.repaint();

			try
			{
				//Thread.sleep(4);
			}
			catch (Exception e)
			{}

			if (_terminate) break;
		}
	}

	private void checkMessages()
	{
		synchronized (_tcpMessages)
		{
			while (!_tcpMessages.isEmpty())
			{
				handleMessage(_tcpMessages.poll());
			}
		}
	}

	private void checkSpawn(double timestamp)
	{
		VehicleInput input = _inputs.peek();
		if (input != null)
		{
			while (input.getTime() < timestamp)
			{
				_inputs.poll();
				doInput(input);
				input = _inputs.peek();
				if (input == null) break;
			}
		}
	}

	private void doInput(VehicleInput input)
	{
		System.out.println("Inputting...");
		BaseVehicle v = input.createEntity();
		_lanes.get(v.getSpawn().getLane().toString()).newVehicle(v);
		_physics.add(v);
		_drawings.add(v);
	}

	private void handleMessage(String msg)
	{
		try
		{
			Object root = _parser.parse(msg);
			JSONDecoder.decodeJSON(root, this);
		}
		catch (ParseException e)
		{
			e.printStackTrace();
		}
	}

	@Override
	public void trafficLightUpdate(Lane which, LightState state)
	{
		System.out.println("Received TLU: " + which.toString() + " = " + state.toString());

		_lanes.get(which.toString()).setLightState(state);
	}

	public TrafficLane getLane(String which)
	{
		return _lanes.get(which);
	}

	public void TCPsend(String what)
	{
		try
		{
			System.out.println("snd: " + what);
			_tcpcon.send(what);
		}
		catch (TCPconnectionException e)
		{
			e.printStackTrace();
		}
	}

	public void detachVehicle(BaseVehicle veh)
	{
		_physics.remove(veh);
		_drawings.remove(veh);
	}
	
	public void multiplierChanged()
	{
		_multiplier = 0x00000001 << _multiplierPow;
		
		JSONObject multiplierObj = new JSONObject();
		multiplierObj.put("multiplier", _multiplier);
		
		System.out.println("New multiplier: " + _multiplier);

		try
		{
			_tcpcon.send("[" + multiplierObj.toJSONString() + "]");
		}
		catch (TCPconnectionException e)
		{
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
	}

	public void incrementMultiplier()
	{
		_multiplierPow++;

		if (_multiplierPow > 10) _multiplierPow = 10;
		
		multiplierChanged();
	}

	public void decrementMultiplier()
	{
		_multiplierPow--;

		if (_multiplierPow < 0) _multiplierPow = 0;
		
		multiplierChanged();
	}

	@Override
	public void keyPressed(KeyEvent arg0)
	{
		// TODO Auto-generated method stub
		System.out.println("key pressed");
	}

	@Override
	public void keyReleased(KeyEvent arg0)
	{
		// TODO Auto-generated method stub
		System.out.println("key released");
		
		switch (arg0.getKeyCode())
		{
			case KeyEvent.VK_MINUS:
				decrementMultiplier();
				break;
			case KeyEvent.VK_EQUALS:
				incrementMultiplier();
				break;
		}
	}

	@Override
	public void keyTyped(KeyEvent arg0)
	{
		
	}
}